#!/usr/bin/env python
# -*- coding:utf-8 -*-
# import ctypes as ct
#
# print (0^0^2^1^0)
import binascii
import serial

x = 0

if __name__ == "__main__":
    print('请保持 imu 静置，程序没有结束前，请不要拔掉电脑端连接 imu 的任何一根线，否则可能导致 imu 损坏。')
    print('')
    try:
        hf_imu = serial.Serial(port='/dev/ttyUSB0', baudrate=921600, timeout=0.5)
        if hf_imu.isOpen():
            print("imu 连接成功等待校准")
        else:
            hf_imu.open()
            print("imu 连接成功等待校准")

    except Exception, e:
        print(e)
        print("imu 默认使用的是 /dev/ttyUSB0 串口号")
        exit(0)
    else:
        hf_imu.write(binascii.a2b_hex('aa551017080000040235aa00000000000000005672'))
        hf_imu.flush()
        while True:
            count = hf_imu.in_waiting
            if count > 11:
                data = hf_imu.read(count)
                data_ = binascii.b2a_hex(data)
                if data_[0:4] == 'aa55':
                    pass
                else:
                    print('校准失败，可能打开了其他串口设备')
                    exit(0)

                if data_[6:8] == '17':
                    print('')
                    print('校准成功')
                    print('')
                    print('请等待 20 s 后，程序自动退出。')
                    print('')
                    print('在退出过程中请不要拔掉电脑端连接 imu 的任何一根线，否则可能导致 imu 损坏。')
                    serial.time.sleep(20)
                    exit(0)
                else:
                    x+=1

                    if x == 2000:
                        print('校验超时')
                        exit(0)

